

#include "sensors_MPL3115.h"
#include "types.h"
#include "driverI2C_ISR.h"
#include "driverLog.h"
#include "sensors.h"
#include "state.h"
#include "config.h"
#include "timers.h"
#include "main_peripherals.h"

// 5 bytes receive buffer for i2c -> 24 bits baro, 16 bits temp
BYTE					baroBuffer[6];
BYTE                                    baroReadRegister;

// i2c action for data reading
struct i2cAction_t		baro;

// i2c action for measurement-command
struct i2cAction_t		baroOneShot;

// value used by baroOneShot
BYTE					baroOSM[2];

TimerHandle_t           baroTimer;


/**
 * Callback from I2C ISR!
 *
 * !! Should not do lots of stuff !!
 */
void baroCallback(portBASE_TYPE * const higherPrior)
{
    if (baro.state != I2C_STATE_DONE)
    {
        STATE.mpl3115.failures++;
        return;
    }

    STATE.mpl3115.totalSamples++;

    STATE.mpl3115.buffer[STATE.mpl3115.bufferPosition] =
        (baroBuffer[0] << 16) | (baroBuffer[1] << 8) | (baroBuffer[2]);

    logStore(xTaskGetTickCountFromISR(), LOG_ID_BARO_RAW, STATE.mpl3115.buffer[STATE.mpl3115.bufferPosition]);

    STATE.mpl3115.bufferPosition++;
    if (STATE.mpl3115.bufferPosition == BARO_BUFFERSIZE) STATE.mpl3115.bufferPosition = 0;


    i2cQueueActionFromISR(&baroOneShot);
}

void baroCallbackError()
{
	
}

/**
 * Configures the MPL3115 output mode, oversampling and mode
 *
 * @return BOOL true if succeeded
 */
BOOL baroConfigure(BYTE baroReg1)
{
    baroOSM[1] = baroReg1;
    struct i2cAction_t * result;
    result = sensorWrite(MPL3115_ADDRESS,0x26,baroReg1);
    if (result->state != I2C_STATE_DONE) return FALSE;

    result = sensorWrite(MPL3115_ADDRESS,0x13,0x07);
    if (result->state != I2C_STATE_DONE) return FALSE;

    return TRUE;
}

/**
 * Checks if MPL3115 who-am-i register is correct
 *
 * @return BOOL true if succeeded
 */
BOOL baroCheck()
{
    struct i2cAction_t * result;
    result = sensorRead(MPL3115_ADDRESS,0x0C,0x00);
    if (result->state == I2C_STATE_DONE && result->readPacket[0] == 0xC4) return TRUE;
    return FALSE;
}

/**
 * Initializes all the necessary MPL3115 registers
 *
 * @return BOOL true if succeeded
 */
BOOL baroInit()
{
    if (!baroCheck()) return FALSE;
    if (!baroConfigure(BARO_CONFIG_MODE_BAROMETER | BARO_CONFIG_ONE_SHOT | BARO_CONFIG_OVERSAMPLE_8)) return FALSE;

    return TRUE;
}

void baroTimerCallback(TimerHandle_t timer)
{
    if (baro.state >= I2C_STATE_DONE && baroOneShot.state >= I2C_STATE_DONE)
    {
        i2cQueueAction(&baro);
        i2cQueueAction(&baroOneShot);
    }
}

/**
 * Called by driverSensors -> sensorInit
 *
 * !! Will block if MPL3115 is not detected !!
 */
void baroSetup()
{
    while (!baroInit())
    {
        BUZZ(BUZZ_DELAY_ERROR, 1);
        BUZZ(BUZZ_DELAY_NOTIFY, 3);
        vTaskDelay(configTICK_RATE_HZ);
    }

    STATE.mpl3115.connected = 1;
    baroReadRegister = 0x00;
    baro.address = MPL3115_ADDRESS;
    baro.writeLength = 1;
    baro.writePacket = &baroReadRegister;
    
    baro.readLength = 6;
    baro.readPacket = baroBuffer;
    baro.callbackFunction = &baroCallback;
    baro.state = I2C_STATE_DONE;

    baroOneShot.address = MPL3115_ADDRESS;
    baroOneShot.writeLength = 2;
    baroOneShot.writePacket = baroOSM;
    baroOneShot.readLength = 0;
    baroOneShot.state = I2C_STATE_DONE;
    baroOSM[0] = 0x26;
    //baroOSM[1] =

    baroTimer = xTimerCreate("", configTICK_RATE_HZ / 25, pdTRUE,NULL, baroTimerCallback);

    if (baroTimer != NULL)
    {
        xTimerStart(baroTimer, 0);
    }
    else
    {
        portNOP();
    }

}
